#include "controller_angPID.h"

// --------------------------------------------------------
// angular PID controller class
// --------------------------------------------------------

// Con/De structors
controller_angPID_c::
controller_angPID_c(float _dT)
{
	Kp 				= 0;
	Ki 				= 0;
	Kd 				= 0;
	upper_bound 	= 0;
	lower_bound 	= 0;
	error			= 0;
	integrator		= 0;
	u_ctrl	 		= 0;
	ref				= 0;
	ref1			= 0;
	feedback		= 0;
	v_feedback		= 0;
	feedback1		= 0;
	error1			= 0;
	dT				= _dT;
}

controller_angPID_c::
controller_angPID_c()
{
	Kp 				= 0;
	Ki 				= 0;
	Kd 				= 0;
	upper_bound 	= 0;
	lower_bound 	= 0;
	error 			= 0;
	integrator		= 0;
	u_ctrl	 		= 0;
	ref				= 0;
	ref1			= 0;
	feedback		= 0;
	v_feedback		= 0;
	feedback1		= 0;
	error1			= 0;
	dT				= 1;
}

controller_angPID_c::
~controller_angPID_c()
{}


//set the parameters of controller
void 
controller_angPID_c::
set(float _Kp, float _Ki, float _Kd, float _upper_bound, float _lower_bound)
{
	Kp 				= _Kp;
	Ki 				= _Ki;
	Kd 				= _Kd;
	upper_bound 	= _upper_bound;
	lower_bound 	= _lower_bound;	
}


//calculate the controller output
int 
controller_angPID_c::
calc()
{
	int iswindup	= 0;	
	error 			= ref - feedback;

	printf("ref = %.4f, feedback = %.4f\n", ref, feedback);
	float abs_error = fabs(error);
	if (abs_error > PI)
	{
		printf("!!error = %.4f\n", error);	
		error = 0 - error / abs_error * (2 * PI - abs_error);		
	}
	printf("error = %.4f\n", error);
	integrator 	   += error * dT;
	
	//printf("error = %.3f, ",error);
	
	if ((u_ctrl >= upper_bound) && (error > 0) && (integrator > 5))
	{
		integrator = PI / 2;
		iswindup = 1;
	}
	else if ((u_ctrl <= lower_bound) && (error < 0) && (integrator < -5))
	{
		integrator = -PI / 2;
		iswindup = -1;
	}	

	//printf("Kp = %.4f, Ki = %.4f, Kd = %.4f\n", Kp, Ki, Kd);
	u_ctrl = Kp * error + Ki * integrator + Kd * (error - error1) / dT;
	
	if (u_ctrl > upper_bound)
	{
		u_ctrl = upper_bound;
	}		
	if (u_ctrl < lower_bound)
	{
		u_ctrl = lower_bound;
	}

	error1 = error;
	//printf("u_ctrl = %.3f\n",u_ctrl);
	
	return iswindup;
}